#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"
#include "ros_demo/CustomMessage.h"
#include <sstream>

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "talker");
    
    // 创建节点句柄
    ros::NodeHandle n;
    
    // 创建发布者
    ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
    ros::Publisher cmd_vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
    ros::Publisher custom_pub = n.advertise<ros_demo::CustomMessage>("custom_topic", 1000);
    
    // 设置发布频率
    ros::Rate loop_rate(10);
    
    int count = 0;
    
    ROS_INFO("Talker节点启动，开始发布消息...");
    
    while (ros::ok())
    {
        // 创建消息
        std_msgs::String msg;
        std::stringstream ss;
        ss << "Hello ROS! 消息编号: " << count;
        msg.data = ss.str();
        
        // 发布字符串消息
        chatter_pub.publish(msg);
        
        // 创建速度控制消息
        geometry_msgs::Twist twist;
        twist.linear.x = 0.5;
        twist.angular.z = 0.3;
        cmd_vel_pub.publish(twist);
        
        // 创建自定义消息
        ros_demo::CustomMessage custom_msg;
        custom_msg.name = "Robot";
        custom_msg.id = count;
        custom_msg.value = count * 0.1;
        custom_msg.position.x = count * 0.1;
        custom_msg.position.y = count * 0.2;
        custom_msg.position.z = 0.0;
        custom_pub.publish(custom_msg);
        
        // 输出日志
        ROS_INFO("发布消息: %s", msg.data.c_str());
        
        // 处理回调函数
        ros::spinOnce();
        
        // 按照设定频率休眠
        loop_rate.sleep();
        ++count;
    }
    
    return 0;
}
